#include "rclcpp/rclcpp.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "rclcpp/rclcpp.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/msg/position_target.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "yolo_detect_message/msg/traker_boxs.hpp"
#include "yolo_detect_message/msg/traker_box.hpp"
#include "yolo_detect_message/msg/detect_boxs.hpp"
#include  "yolo_detect_message/msg/detect_box.hpp"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <thread>
#include <algorithm>
#include <mutex>
#include "yolo_detect_message/srv/service.hpp"
#include "yolo_detect_message/msg/call_server_status.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "quyufugai/GenearatorPoint.hpp"
#include "yolo_detect_message/msg/move_interface.hpp"
#include "yolo_detect_message/msg/call_server_status.hpp"
#include <queue>

class quyufugai:public rclcpp::Node
{
private:
    double errs;
    double speed;
    // 无人机数量
    int vehicle_num;
    // 控制类型，vel或accel
    std::string control_type;
    std::string namespaces;
    std::string vehicle_type;
    std::vector<Path> point_list;
    double top_left_x;
    double top_left_y;

    double width ;
    double height ;
    double spacing;
    double gap ;
    // 订阅无人机当前的位姿
    std::vector<geometry_msgs::msg::PoseStamped> pose;
    // 无人机位置信息的订阅者
    std::vector<rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr> pose_sub;
    ZigzagPathPlanner path_genearator;
    rclcpp::TimerBase::SharedPtr timer_;
    std::mutex server_status_mutex_;
    bool service_status = false;

    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_sub_;
    std::vector<rclcpp::Publisher<yolo_detect_message::msg::MoveInterface>::SharedPtr> move_interface_pubs;
    std::mutex move_interface_mutex_;
    // 
    std::vector<Point> point_list_;
    std::vector<yolo_detect_message::msg::MoveInterface> move_interfaces;
    std::vector<std::queue<Point>> point_list_queue;
    std::vector<bool> arrived_first;
    std::vector<bool> first_received;

    
public:
    
    void initialize_parameters();
    quyufugai(/* args */);
    ~quyufugai(){}
    void controlLoop();
    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
   
    bool is_arrival(const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2, double threshold = 0.5) ;
};

